/*
 * GY80.c
 *
 *  Created on: 10-12-2012
 *      Author: Maciej
 */

#include "GY80.h"
#include "my_i2c.h"
#include "math.h"

#define RADTODEG (360.f/6.28318531f)
inline float toDegrees(double x)
{
	return (float) (x * RADTODEG);
}

//struct sensorStruct sensor_data;

void GY80_init(sSensors* sensors)
{
	sensors->gX = 0;
	sensors->gY = 0;
	sensors->gZ = 0;
	sensors->aX = 0;
	sensors->aY = 0;
	sensors->aZ = 0;
	sensors->Pressure = 0;
	sensors->mX = 0;
	sensors->mY = 0;
	sensors->mZ = 0;
	sensors->Temperature = 0; // ReadRegister(BMP_ADDR,BMP_TEMPERATURE_REG);

	//volatile unsigned char X=0;
	// First check for errors

//	printf("Gyro %s present\n", TI_USCI_I2C_slave_present(L3G_ADDR)?"is":"not");
//	printf("Gyro chip ID test %s\n", TI_i2c_read_register(L3G_ADDR,L3G_WHO_AM_I)==L3G_I_AM?"OK":"FAIL");
	//WriteRegister(L3G_ADDR, L3G_CTRL_REG1, 0x8F);
	//WriteRegister(L3G_ADDR, L3G_CTRL_REG1, 0xFF);
	WriteRegister(L3G_ADDR, L3G_CTRL_REG1, 0x0F);
	//WriteRegister(L3G_ADDR, L3G_CTRL_REG2, 0x28);
	WriteRegister(L3G_ADDR, L3G_CTRL_REG2, 0x00);
	WriteRegister(L3G_ADDR, L3G_CTRL_REG3, 0x00);
	//WriteRegister(L3G_ADDR, L3G_CTRL_REG4, 0x40);
	//WriteRegister(L3G_ADDR, L3G_CTRL_REG4, 0x70);
	WriteRegister(L3G_ADDR, L3G_CTRL_REG4, 0x40);
	WriteRegister(L3G_ADDR, L3G_CTRL_REG5, 0x00);
	//printf("Gyro set\n");

//	printf("Acc %s present\n",  TI_USCI_I2C_slave_present(ADX_ADDR)?"is":"not");
//	printf("Acc chip ID test %s\n", TI_i2c_read_register(ADX_ADDR,ADX_DEVID_REG)==ADX_DEVID?"OK":"FAIL");
	WriteRegister(ADX_ADDR, ADX_BW_RATE_REG, ADX_BW_12);
	WriteRegister(ADX_ADDR, ADX_POWER_CTL_REG, 0x08);
	WriteRegister(ADX_ADDR, ADX_DATA_FORMAT_REG, 0x09);
	//printf("Acc set\n");

	//printf("Mag %s present\n",TI_USCI_I2C_slave_present(HMC_ADDR)?"is":"not");
	//printf("Mag chip ID test %s\n", TI_i2c_read_register(HMC_ADDR,HMC_ID_REG)==HMC_ID?"OK":"FAIL");
	WriteRegister(HMC_ADDR, HMC_A_REG, 1 << 5);
	WriteRegister(HMC_ADDR, HMC_B_REG, 1 << 5);
	WriteRegister(HMC_ADDR, HMC_MODE_REG, 0x00);
	//printf("Mag set\n");

	//printf("Pre %s present\n",TI_USCI_I2C_slave_present(BMP_ADDR)?"is":"not");
	//printf("Pre chip ID test %s\n", TI_i2c_read_register(BMP_ADDR,BMP_CHIP_ID_REG)==BMP_CHIP_ID?"OK":"FAIL");
	//printf("Mag set\n");

}

//#define GYRO_SCALE 2000.0f
#define GYRO_SCALE 250.0f

void GY80_Update(sSensors* sensors)
{
	sensors->gX = ReadRegisterW(L3G_ADDR, L3G_OUT_X_L) / GYRO_SCALE;	// 250.0f;
	sensors->gY = ReadRegisterW(L3G_ADDR, L3G_OUT_Y_L) / GYRO_SCALE;	//250.0f;
	sensors->gZ = ReadRegisterW(L3G_ADDR, L3G_OUT_Z_L) /GYRO_SCALE;	//250.0f;

	//WriteRegister(BMP_ADDR,BMP_CTRL_MEAS_REG,(3<<6)|BMP_PRESSURE);

	sensors->aX = ReadRegisterW(ADX_ADDR, ADX_X_REG) / 250.0f;
	sensors->aY = ReadRegisterW(ADX_ADDR, ADX_Y_REG) / 250.0f;
	sensors->aZ = ReadRegisterW(ADX_ADDR, ADX_Z_REG) / 250.0f;

	//sensor_data.Pressure = ReadRegister(BMP_ADDR,BMP_PRESSURE_REG);
	//WriteRegister(BMP_ADDR,BMP_CTRL_MEAS_REG,(3<<6)|BMP_TEMPERATURE);

//	sensors->mX = ReadRegisterW(HMC_ADDR, HMC_X_REG);
//	sensors->mY = ReadRegisterW(HMC_ADDR, HMC_Y_REG);
//	sensors->mZ = ReadRegisterW(HMC_ADDR, HMC_Z_REG);

//	UARTprintf(
//			"~,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n",
//			sensors->aX,
//			sensors->aY,
//			sensors->aZ,
//			sensors->gX,
//			sensors->gY,
//			sensors->gZ,
//			sensors->mX,
//			sensors->mY,
//			sensors->mZ,
//			sensors->Pressure,
//			sensors->Temperature);

	//sensor_data.Temperature = ReadRegister(BMP_ADDR,BMP_TEMPERATURE_REG);
}

// returns angle form acc reading (unfildered) in degrees
float GY80_ReadAngleAccZX(sSensors* sensors)
{
	//UARTprintf("aZ aX:%i,%i\n", sensors->aZ, sensors->aX);
	//return
	float test = toDegrees(atan2((double) sensors->aZ, (double) sensors->aX) + 1.57079633f);
	return test-3; // corection for hardware configuration
}
